#include "defines.h"
#include "serial.h"
#include "assembly.h"
#include "joystick_button.h"

void _dummy_function() {
	asm ("nop");
}

int main(void) {
	unsigned int address, data;
	unsigned char val, ldata;
	void (*funcptr)(void) = 0x00000; // Set up function pointer to RESET vector

	//init
	uart_init(56700);
	button_init();
	scheduler_init();
	set_pin_HIGH();
	button_set_Status_Pin(set_pin_LOW);

	/*
	 * after pressing the reset this code lets the user press the joystick
	 * to flash a new program into the flash, if the button is pressed
	 * the PORTD5 is set to 0 and therefore begin receiving commands from
	 * the AVRprog.
	 */
	scheduler_add(_dummy_function, WAIT_MS);

	if (PROGMODE == 1) { 	// the user did not pressed the joystick

		funcptr();

	} else { 				// Otherwise some programming

		bool programming_mode = true;
		while (programming_mode) {
			val = recCommand();

			if (val == 'a') {	//Autoincrement adress


			} else if (val == 'b') {

			} else if (val == 'A') { // set address

				address = recCommand();
				address = (address << 8) | recCommand();

				address = address << 1; // convert from word address to byte address
				sendCommand('\r');

			} else if (val == 'e') { // Chip erase

				for (address = 0; address < APP_END; address += PAGESIZE) {
					boot_page_erase(address);
				}
				sendCommand('\r');

			} else if (val == 'E') {

				sendCommand('e');

			} else if (val == 'L') { // Enter programming mode

				sendCommand('\r');

			} else if (val == 'g') { // Start Block Flash Read

			} else if (val == 'B') { // load Block

				boot_spm_interrupt_disable();
				for (int i = 0x00000; i < PAGESIZE; i++) {
					data = ldata | (recCommand() << 8);
					fill_temp_buffer(data, (address)); //call asm routine.
					address += 2;
					boot_page_fill(i, data);
					sendCommand('C');
				}

				boot_spm_interrupt_enable();

			} else if (val == 't') { // Return programmer type

				sendCommand(PARTCODE);
				sendCommand(0);

			} else if (val == 'P') {

			} else if (val == 'S') { // Return software identifier

				sendCommand('A');
				sendCommand('V');
				sendCommand('R');
				sendCommand('B');
				sendCommand('O');
				sendCommand('O');
				sendCommand('T');

			} else if (val == 'T') {

			} else if (val == 'V') { // Return Software Version

				sendCommand('1');
				sendCommand('0');

			} else if (val == 's') { // Return Signature Byte

				sendCommand(SIGNATURE_BYTE_1);
				sendCommand(SIGNATURE_BYTE_2);
				sendCommand(SIGNATURE_BYTE_3);

			} else { // if not reconized
				sendCommand('?');
			}

		} // end while (1)

		// jump to program
		funcptr();

		/* NOTREACHED */

	} // if joystick pressed
	return 0;
}
